﻿using System;
using System.Collections.Generic;
using System.Text;
using LocalizationTest.Tools;
using LocalizationTest.SelfLocators.MBL;

namespace LocalizationTest
{
  class MBLSimpleGridRefinement2 : SelfLocator
  {
    double[] minimum;
    double[] maximum;
    double[] step;

    double[] lastBestWorldstate = null;

    public override WorldState Execute(Experiment experiment)
    {
      if (experiment.actionAndObservationHistory.history.Count == 0) return null;
      List<ActionAndObservationHistory.RelativePlaceWithMeasurement> listOfOldPlaces = experiment.actionAndObservationHistory.GetSelectedRelativePlaces(experiment.simulatedWorldState);
      if (listOfOldPlaces.Count == 0) return null;

      minimum = Globals.GetMinimum(experiment.simulatedWorldState).ToDoubleArray();
      maximum = Globals.GetMaximum(experiment.simulatedWorldState).ToDoubleArray();
      step = Globals.GetStep(experiment.simulatedWorldState).ToDoubleArray();

      ResetRangeAndStepWidth(experiment.simulatedWorldState.ToDoubleArray(), 0.25);
      step = Globals.GetStep(experiment.simulatedWorldState).ToDoubleArray();

      int numberOfRefinements = 1;
      if (DebugKeyTool.IsActive("MBLSimpleGridRefinement2: multiple refinements")) numberOfRefinements = 6;

      double minimalValue;
      double maximalValue;
      double[] positionOfMinimalValue = new double[3];
      double[] positionOfMaximalValue = new double[3];

      SimilarityFunction similarityFunction = new SimilarityFunction(listOfOldPlaces, experiment, false);
      SimilarityFunction similarityFunctionLastMeasurementOnly = new SimilarityFunction(listOfOldPlaces, experiment, true);

      for (int i = 0; i < numberOfRefinements; i++)
      {
//        similarityFunctionLastMeasurementOnly.FindMinimalRange(5000, minimum, maximum, step);
        similarityFunction.FindMinium(out minimalValue, out positionOfMinimalValue, out maximalValue, out positionOfMaximalValue,
          minimum, maximum, step);
        ResetRangeAndStepWidth(positionOfMinimalValue, 0.5);
      }
      lastBestWorldstate = positionOfMinimalValue;
      WorldState toReturn = (WorldState)System.Activator.CreateInstance(experiment.simulatedWorldState.GetType());
      toReturn.FromDoubleArray(positionOfMinimalValue);

/*
      TimeTool.Start("500 runs");
      double[] vec = new double[3]{0,0,0};
      for (int i = 0; i < 1000; i++)
      {
        vec[0] = i;
        similarityFunction.Evaluate(vec);
      }
      TimeTool.Stop("500 runs");
*/
      return toReturn;
    }

    private void ResetRangeAndStepWidth(double[] positionOfMinimalValue, double refinementFactor)
    {
      for (int i = 0; i < positionOfMinimalValue.Length; i++)
      {
        double range = maximum[i] - minimum[i];
        range *= refinementFactor;
        minimum[i] = positionOfMinimalValue[i] - range / 2.0;
        maximum[i] = positionOfMinimalValue[i] + range / 2.0;
        step[i] *= refinementFactor;
      }
    }
  }
}
